#include <nrk.h>
#include <include.h>
#include <ulib.h>
#include <stdio.h>
#include <avr/sleep.h>
#include <hal.h>
//#include <rt_link.h>
#include <nrk_error.h>
#include <nrk_events.h>
#include <nrk_driver.h>
#include <nrk_driver_list.h>
#include <ff_basic_sensor.h>
#include  <math.h>
#include<bmac.h>


#define MY_CHANNEL 17
#define MOLE_ID 1

NRK_STK StackRX[NRK_APP_STACKSIZE];
nrk_task_type TaskRX;
void Task_RX(void);

NRK_STK StackTX[NRK_APP_STACKSIZE];
nrk_task_type TaskTX;
void Task_TX(void);

NRK_STK StackMole[NRK_APP_STACKSIZE];
nrk_task_type TaskMole;
void Task_Mole(void);

void nrk_create_taskset();
uint8_t tx_buf[RF_MAX_PAYLOAD_SIZE];
uint8_t rx_buf[RF_MAX_PAYLOAD_SIZE];

nrk_time_t timestart;
nrk_time_t curtime;
uint32_t timeout = 4;

uint8_t is_mole_whacked = '0';
int16_t moleID = -1;


int
main ()
{
  nrk_setup_ports();
  nrk_setup_uart(UART_BAUDRATE_115K2);
	
  nrk_kprintf( PSTR("Starting up...\r\n") );
	
  nrk_init();
	
  nrk_led_clr(0);
  nrk_led_clr(1);
  nrk_led_clr(2);
  nrk_led_clr(3);
	
  nrk_time_set(0,0);
	
  nrk_register_drivers();
  bmac_task_config ();
  nrk_create_taskset ();
	
  nrk_start();
	
  return 0;
}

void Task_Mole(){
  uint8_t fd,val;
  uint16_t light = 0;
  uint16_t threshold;
	
  printf( "Task_TX PID=%d\r\n",nrk_get_pid());
  printf( "Node ID=%d\r\n",MOLE_ID);
	
  /* setup sensor */
  fd=nrk_open(FIREFLY_SENSOR_BASIC,READ);
  if(fd==NRK_ERROR) nrk_kprintf(PSTR("Failed to open sensor driver\r\n"));
	
  val=nrk_set_status(fd,SENSOR_SELECT,LIGHT);

  threshold = 900;

  while(1){
    if(moleID == MOLE_ID){
      val = nrk_read( fd, &light, 2 );
      if ( light > threshold ){
	is_mole_whacked = '1';
	nrk_led_clr(RED_LED);
      }
    }
    nrk_wait_until_next_period();
  }
}

void Task_RX()
{
  uint8_t j, i;
  uint8_t rssi,length;
  uint8_t *local_rx_buf;
	
  printf( "Task_RX PID=%d\r\n",nrk_get_pid());
  printf( "Node ID=%d\r\n",MOLE_ID);
	
  /* start bmac task */
  bmac_init (MY_CHANNEL);
  bmac_rx_pkt_set_buffer (rx_buf, RF_MAX_PAYLOAD_SIZE);
  while (!bmac_started ())
    nrk_wait_until_next_period ();
	
		
  /* setup mole id */
  tx_buf[8] = MOLE_ID + 48;
	
	
  while(1) {
		
    /* wait until packet is received and then process packet */
    if(!bmac_rx_pkt_ready())
      j = bmac_wait_until_rx_pkt ();
    nrk_led_set(BLUE_LED);
    nrk_led_clr(RED_LED);
    printf( "here\r\n" );
    local_rx_buf = bmac_rx_pkt_get (&length, &rssi);
		
    for(i=0; i<length; i++ )
      printf( "%c",local_rx_buf[i] );
		
    moleID = local_rx_buf[34]-48;
    if(moleID == MOLE_ID){
      nrk_led_set(RED_LED);
      is_mole_whacked = '0';
    }
		
    printf(" rounds value  %d\r\n ", local_rx_buf[48]-48);
    printf("NEW: %c",local_rx_buf[34]);
    nrk_kprintf( PSTR("\r\n") );
    bmac_rx_pkt_release();
    nrk_led_clr(BLUE_LED);
		
  }
}


void Task_TX()
{
  nrk_sig_t tx_done_signal;
  uint8_t ret;
	
  tx_done_signal = bmac_get_tx_done_signal (); //get signal for tx done
  nrk_signal_register (tx_done_signal); //register the task to wait signal
	
	
  printf( "Task_TX PID=%d\r\n",nrk_get_pid());
  printf( "Node ID=%d\r\n",MOLE_ID);
	
  while (!bmac_started ())
    nrk_wait_until_next_period ();
	
  while(1) {
    printf("Got mole id: %d\r\n", moleID);
    if(is_mole_whacked == '1'){
      printf("I am mole\r\n");
      nrk_led_clr(GREEN_LED);
      tx_buf[16] = is_mole_whacked;
      bmac_tx_pkt_nonblocking(tx_buf, 18);
      ret = nrk_event_wait (SIG(tx_done_signal));
      printf("Transmitted packet");
      nrk_led_clr(GREEN_LED);
      is_mole_whacked = '0';
    }
    else{
      nrk_wait_until_next_period();
    }
		
  }
}

void
nrk_create_taskset()
{
	
	
  TaskRX.task = Task_RX;
  TaskRX.Ptos = (void *) &StackRX[NRK_APP_STACKSIZE-1];
  TaskRX.Pbos = (void *) &StackRX[0];
  TaskRX.prio = 3;
  TaskRX.FirstActivation = TRUE;
  TaskRX.Type = BASIC_TASK;
  TaskRX.SchType = PREEMPTIVE;
  TaskRX.period.secs = 0;
  TaskRX.period.nano_secs =  500*NANOS_PER_MS;
  TaskRX.cpu_reserve.secs = 0;
  TaskRX.cpu_reserve.nano_secs = 100*NANOS_PER_MS;
  TaskRX.offset.secs = 0;
  TaskRX.offset.nano_secs= 0;
  nrk_activate_task (&TaskRX);
	
  TaskTX.task = Task_TX;
  TaskTX.Ptos = (void *) &StackTX[NRK_APP_STACKSIZE-1];
  TaskTX.Pbos = (void *) &StackTX[0];
  TaskTX.prio = 2;
  TaskTX.FirstActivation = TRUE;
  TaskTX.Type = BASIC_TASK;
  TaskTX.SchType = PREEMPTIVE;
  TaskTX.period.secs = 0;
  TaskTX.period.nano_secs = 500*NANOS_PER_MS;
  TaskTX.cpu_reserve.secs = 0;
  TaskTX.cpu_reserve.nano_secs = 100*NANOS_PER_MS;
  TaskTX.offset.secs = 0;
  TaskTX.offset.nano_secs= 0;
  nrk_activate_task (&TaskTX);
	
  TaskMole.task = Task_Mole;
  TaskMole.Ptos = (void *) &StackMole[NRK_APP_STACKSIZE-1];
  TaskMole.Pbos = (void *) &StackMole[0];
  TaskMole.prio = 4;
  TaskMole.FirstActivation = TRUE;
  TaskMole.Type = BASIC_TASK;
  TaskMole.SchType = PREEMPTIVE;
  TaskMole.period.secs = 0;
  TaskMole.period.nano_secs = 200*NANOS_PER_MS;
  TaskMole.cpu_reserve.secs = 0;
  TaskMole.cpu_reserve.nano_secs = 100*NANOS_PER_MS;
  TaskMole.offset.secs = 0;
  TaskMole.offset.nano_secs= 0;
  nrk_activate_task (&TaskMole);
	
	
	
  nrk_kprintf( PSTR("Create Done\r\n") );
}


void nrk_register_drivers()
{
  int8_t val;
	
  // Register the Basic FireFly Sensor device driver
  // Make sure to add: 
  //     #define NRK_MAX_DRIVER_CNT  
  //     in nrk_cfg.h
  // Make sure to add: 
  //     SRC += $(ROOT_DIR)/src/drivers/platform/$(PLATFORM_TYPE)/source/ff_basic_sensor.c
  //     in makefile
  val=nrk_register_driver( &dev_manager_ff_sensors,FIREFLY_SENSOR_BASIC);
  if(val==NRK_ERROR) nrk_kprintf( PSTR("Failed to load my ADC driver\r\n") );
	
}

